#include "ares_pathing/aresMap.h"
#include "ares_pathing/path.h"

//MAIN CONSTRUCTOR
aresMap::aresMap(nav_msgs::OccupancyGrid msg){
    botflag = false;
    goalflag = false;    
    ROS_INFO("Making Map!");
    //grab all of the meta data            
    load_time = msg.info.map_load_time;
    res = msg.info.resolution;
    w = msg.info.width;
    h = msg.info.height;
    tf::poseMsgToTF(msg.info.origin,origin);

    //make the row-major listing into a 2d array (x,y)
    for(unsigned int i=0;i<w*h;i++){
        grid.push_back(msg.data[i]);
    }
    ROS_INFO("Map Made :) Grid size %d",grid.size());
} //end constructor

//Destructor
aresMap::~aresMap(void){}

/*Preconditions: a occupancy grid msg that represents a new map
Postconditions: the map, bot and goal locations are updated for the new map
*/
void aresMap::updateMap(nav_msgs::OccupancyGrid msg){
    //save old data to update bot/goal info
    float oldx = origin.getOrigin().getX();
    float oldy = origin.getOrigin().getY();

    //grab all of the meta data            
    load_time = msg.info.map_load_time;
    res = msg.info.resolution;
    w = msg.info.width;
    h = msg.info.height;
    tf::poseMsgToTF(msg.info.origin,origin);

    //make the row-major listing into a 2d array (x,y)
    grid.clear();
    for(unsigned int i=0;i<w*h;i++){
        grid.push_back(msg.data[i]);
    }

    //update the bot location
    bot.x = bot.x-(oldx-origin.getOrigin().getX());
    bot.y = bot.y-(oldy-origin.getOrigin().getY());
    bot.xCell = metersToIndex(bot.x);
    bot.yCell = metersToIndex(bot.y);

    //update the goal location
    goal.x = goal.x-(oldx-origin.getOrigin().getX());
    goal.y = goal.y-(oldy-origin.getOrigin().getY());
    goal.xCell = metersToIndex(goal.x);
    goal.yCell = metersToIndex(goal.y);

    //announce complete
    ROS_INFO("Map updated :) Grid size %d",grid.size());
}

/*Preconditions: An odometry message of the robot position to update
Postconditions: The saved position, orientation and velocity of the robot in the map is updated.
*/
void aresMap::updateBot(nav_msgs::Odometry odom){
    //save the pose of the bot for use in this function
    tf::Pose botLoc;
    tf::poseMsgToTF(odom.pose.pose,botLoc);
    bot = poseToCell(botLoc);
    //save the current linear velocity of the bot
    bot.vel = odom.twist.twist.linear.x;
    ROS_INFO("Updated Bot! Linear Velocity: %f",bot.vel);
}

/*Preconditions: a Pose representing a desired location
Postconditions: the pose is saved as the current goal
*/
void aresMap::setGoal(geometry_msgs::Pose goalIn){
    tf::Pose g;
    tf::poseMsgToTF(goalIn, g);
    goal = poseToCell(g);
    ROS_INFO("Goal Set @ (%d,%d)",goal.xCell,goal.yCell);

}

/*Preconditions: A pose within the odom frame
Postoconditions: A posInfo with all relevant position information is returned
*/
cell aresMap::poseToCell(tf::Pose p){    
    cell tmp;    
    //save the XY position in meters, within the map
    tmp.x = p.getOrigin().getX()-origin.getOrigin().getX();
    tmp.y = p.getOrigin().getY()-origin.getOrigin().getY();
    //save the xy position of the bot in cell coords
    tmp.xCell = metersToIndex(tmp.x);
    tmp.yCell = metersToIndex(tmp.y);
    //save the heading of the bot, with respect to odom frame
    tmp.heading = tf::getYaw(p.getRotation());
    return tmp;
}

/*Preconditions: a distance in the map
  Postconditions: the # of cells is returned
*/
int aresMap::metersToIndex(float m){
    return round(m/res);
}

void aresMap::layPath(){    
    //get the offset goal position
    cell goalb = offsetBackLinear(goal);
    //check that the offset goal is valid
    if(!footprintCheck(goalb)){
        ROS_WARN("Offset goal position is not valid. Stopping Planning");        
        return;
    }
    //recursively search for the sequence of cells that leads to the goal
    vector<Path> paths;
    vector<int> indicies = checkNeighbors(bot,goalb,&paths); //get all of the paths in the map...
    //add the start to all of the paths to end them
    for(int i=0;i<indicies.size();i++){
        paths[indicies[i]].setStart(bot);
    }
    ROS_INFO("FOUND %i PATHS!",indicies.size());
    //set the scores for all of the paths and pick the best one
    float s, min=1e9;
    int choice=0;
    for(int i=0;i<paths.size();i++){
        s=paths[i].calcScore(goal);
        if(s<min){
            min = s;
            choice = i;
            ROS_INFO("New min path score - Path %i: %f",choice,min);
        }
    }
    road = &paths[choice];
    road->setVel();
}

/*Preconditions: the coordinates of a cell in the map, the goal, and the vector of paths, and the index of the current path
Postconditions: returns a vector of the path indexes for which this cell is included. Recursively calls for its valid neighbors before returning.
*/   
vector<int> aresMap::checkNeighbors(cell c, cell g, vector<Path> * p){
    ROS_INFO("Checking Neighbors for (%i,%i)",c.xCell,c.yCell);    
    vector<int> indicies,i_tmp;
    //loop through all of the neighbors
    for(int x=-1;x<=1;x++){
        for(int y=-1;y<=1;y++){
            if(x==0 && y==0) continue; //skip yourself.
            //get all of the paths the neighbor is in
            i_tmp = checkNeighbor(c, makeNeighborCell(c, x, y),g,p);                
            //add those paths to those of the other neighbors
            for(int i=0;i<i_tmp.size();i++){
                indicies.push_back(i_tmp[i]);
            }
            ROS_INFO("# paths: %d",indicies.size());
        }
    }
    return indicies;    //send back all of the paths that this cell should be added to.
}

/*Preconditions: the cell who is checking its neighbor, the neighbor, the map goal, and a pointer to the array of paths
  Postconditions: the cell checks its status and creates a path or checks its neighbors accordingly. It adds itself to the proper paths and returns the indicies of the paths to which it belongs.
*/
vector<int> aresMap::checkNeighbor(cell c, cell n, cell g, vector<Path> * p){
    vector<int> indicies;    
    if(abs(n.heading-c.heading)>=maxHeadingChange){ //check for within heading range
        ROS_INFO("(%d,%d) is out of heading range",n.xCell,n.yCell);       
        return indicies; //return w/ nothing in it
    }
    if(!footprintCheck(n)){ //check for robot footprint
        ROS_INFO("(%d,%d) has footprint failure",n.xCell,n.yCell);       
        return indicies; //return w/ nothing in it
    }
    if(grid[coordToIndex(n.xCell,n.yCell)]==-1 || ((n.xCell==g.xCell) && (n.yCell == g.yCell))){ //is unknown or goal
        ROS_INFO("(%d,%d) starts a new path",n.xCell,n.yCell);       
        //start a new path!        
        p->push_back(Path(n));
        indicies.push_back(p->size()-1);
        return indicies; //return the new index!
    }
    //just an ordinary, average cell. Need to check its neighbors.
    indicies = checkNeighbors(n,g,p);
    //add this cell to any paths the neighbors were included in
    for(int i=0;i<indicies.size();i++){
        p->at(indicies[i]).addCell(n.xCell,n.yCell);
    }
    return indicies; //return the indicies list for this cell so others can be in these paths.
}

cell aresMap::offsetBackLinear(cell c){
    cell g;
    g.heading = c.heading;
    g.x = c.x-goalOffset*sin(c.heading);
    g.y = c.y-goalOffset*cos(c.heading);
    g.xCell = metersToIndex(g.x);
    g.yCell = metersToIndex(g.y);
    return g;
}

/*Preconditions: a cell in the grid with a defined heading representing the direct of the robot
  Postconditions: true is returned if the bot will fit with the defined heading, else false
*/
bool aresMap::footprintCheck(cell c){
        int x,y;
        //check all of the cells that should intersect the frame of the robot
        for(float i=0.0;i<botL;i+=res){ //x-dir
            for(float j=0.0;j<botW;j+=res){ //y-dir
                x = cos(c.heading)*(botFrontOffset-i)-sin(c.heading)*(0.5*botW-j)+c.x;
                y = sin(c.heading)*(botFrontOffset-i)+cos(c.heading)*(0.5*botW-j)+c.y;
                ROS_INFO("Footprint: (%d,%d)m i:%f j:%f",x,y,i,j);
                x = x/res;
                y = y/res;
                if(!spotCheck(x,y)){
                    ROS_INFO("Footprint check failed");
                    return false;
                }
            }
        }
        ROS_INFO("Footprint check passed");
        return true;
}

/*Preconditions: a cell to make a neighbor for, the dx & dy (in cells) to the neighbor from the cell
  Postconditions: a cell for the neighbor is returned, where the heading is the direction from the old cell to the new cell.
*/
cell aresMap::makeNeighborCell(cell c, int dx, int dy){
    cell z;
    z.xCell = c.xCell+dx;
    z.yCell = c.yCell+dy;
    z.x = c.x+dx*res;
    z.y = c.y+dy*res;
    z.heading = atan2(dy,dx);
    return z;
}


void aresMap::writeMsg(drive_commands::twist_path * m){
    ROS_INFO("Writing Message!");
    m->res = res;
    m->vel_max = Path::maxVel;
    m->skipframes = 1;
    ROS_INFO("Message setting current");
    m->current.vel = bot.vel;
    m->current.globalrad = bot.heading;
    m->current.deltarad = 0;

    //have the path determine the velocities for each of the next five points
    path_point future[5];
    road->pathPointGen(1,future);
    ROS_INFO("Recieved the future");
    m->next1 = future[0];
    m->next2 = future[1];
    m->next3 = future[2];
    m->next4 = future[3];
    m->next5 = future[4];
    ROS_INFO("m totally filled");
}

//FOR DEBUGGING
void aresMap::printBot(){
    ROS_INFO("Bot loc: (%f, %f) m - Cell: (%d,%d) - Heading: %f",bot.x,bot.y,bot.xCell,bot.yCell,bot.heading);
}

void aresMap::printGoal(){
    ROS_INFO("Goal loc: (%f, %f) m - Cell: (%d,%d) - Heading: %f",goal.x,goal.y,goal.xCell,goal.yCell,goal.heading);
}

void aresMap::printWorld(){
    ROS_INFO("Map Res: %f, Width: %d, Height: %d, Origin @ (%f,%f)",res,w,h,origin.getOrigin().getX(),origin.getOrigin().getY());
}
